import ctypes
import time

# 加载接口库（确保ControlCANFD.dll与脚本同目录）
canDLL = ctypes.windll.LoadLibrary("./ControlCANFD.dll")

# -------------------------- 常量定义（参考手册3.1、6.2.1节及例程） --------------------------
# 设备类型（例程中USBCANFD设备类型为41，与手册6.2.1节示例一致）
VCI_USBCAN2 = 41  
# 状态码（例程中STATUS_OK=1，与库函数返回值定义匹配）
STATUS_OK = 1  
STATUS_ERR = 0
# 无效句柄（参考手册3.1节）
INVALID_DEVICE_HANDLE = 0
INVALID_CHANNEL_HANDLE = 0
# 帧类型（手册2.2节）
TYPE_CAN = 0       # 普通CAN
TYPE_CANFD = 1     # CANFD
# 滤波模式（手册3.22节）
FILTER_MODE_STD = 0  # 标准帧
FILTER_MODE_EXT = 1  # 扩展帧

# -------------------------- 数据结构定义（严格遵循手册2.2-2.8节及例程） --------------------------
# CAN通道初始化配置（手册2.2节 ZCAN_CHANNEL_INIT_CONFIG）
class _ZCAN_CHANNEL_CAN_INIT_CONFIG(ctypes.Structure):
    _fields_ = [("acc_code", ctypes.c_uint),    # 验收码
                ("acc_mask", ctypes.c_uint),    # 屏蔽码
                ("reserved", ctypes.c_uint),    # 保留
                ("filter", ctypes.c_ubyte),     # 滤波方式（0=双滤波，1=单滤波）
                ("timing0", ctypes.c_ubyte),    # 忽略
                ("timing1", ctypes.c_ubyte),    # 忽略
                ("mode", ctypes.c_ubyte)]       # 工作模式（0=正常，1=只听）

class _ZCAN_CHANNEL_CANFD_INIT_CONFIG(ctypes.Structure):
    _fields_ = [("acc_code", ctypes.c_uint),     # 验收码
                ("acc_mask", ctypes.c_uint),     # 屏蔽码
                ("abit_timing", ctypes.c_uint),  # 忽略
                ("dbit_timing", ctypes.c_uint),  # 忽略
                ("brp", ctypes.c_uint),          # 波特率预分频（设0）
                ("filter", ctypes.c_ubyte),      # 滤波方式
                ("mode", ctypes.c_ubyte),        # 工作模式
                ("pad", ctypes.c_ushort),        # 数据对齐
                ("reserved", ctypes.c_uint)]     # 保留

# 联合结构体（手册2.2节 union）
class _ZCAN_CHANNEL_INIT_CONFIG(ctypes.Union):
    _fields_ = [("can", _ZCAN_CHANNEL_CAN_INIT_CONFIG),
                ("canfd", _ZCAN_CHANNEL_CANFD_INIT_CONFIG)]

# 通道初始化总配置（手册2.2节）
class ZCAN_CHANNEL_INIT_CONFIG(ctypes.Structure):
    _fields_ = [("can_type", ctypes.c_uint),          # 设备类型（TYPE_CAN/TYPE_CANFD）
                ("config", _ZCAN_CHANNEL_INIT_CONFIG)] # 联合配置

# CAN帧结构（手册2.3节 can_frame）
class ZCAN_CAN_FRAME(ctypes.Structure):
    _fields_ = [("can_id", ctypes.c_uint, 29),  # 帧ID（低29位）
                ("err", ctypes.c_uint, 1),     # 错误帧标志（0=正常）
                ("rtr", ctypes.c_uint, 1),     # 远程帧标志（0=数据帧）
                ("eff", ctypes.c_uint, 1),     # 扩展帧标志（0=标准帧，1=扩展帧）
                ("can_dlc", ctypes.c_ubyte),   # 数据长度（0-8）
                ("__pad", ctypes.c_ubyte),     # 对齐
                ("__res0", ctypes.c_ubyte),    # 保留
                ("__res1", ctypes.c_ubyte),    # 保留
                ("data", ctypes.c_ubyte * 8)]  # 数据域

# 发送数据结构（手册2.5节 ZCAN_Transmit_Data）
class ZCAN_Transmit_Data(ctypes.Structure):
    _fields_ = [("frame", ZCAN_CAN_FRAME),          # CAN帧
                ("transmit_type", ctypes.c_uint)]    # 发送方式（0=正常发送）

# 接收数据结构（手册2.7节 ZCAN_Receive_Data）
class ZCAN_Receive_Data(ctypes.Structure):
    _fields_ = [("frame", ZCAN_CAN_FRAME),          # CAN帧
                ("timestamp", ctypes.c_ulonglong)]   # 时间戳（微秒）

# -------------------------- 函数参数类型声明（参考手册3.1-3.25节及例程） --------------------------
# 打开设备（手册3.1节 ZCAN_OpenDevice）
canDLL.ZCAN_OpenDevice.restype = ctypes.c_void_p
canDLL.ZCAN_OpenDevice.argtypes = (ctypes.c_uint, ctypes.c_uint, ctypes.c_uint)

# 设置CANFD仲裁域波特率（手册3.16节 ZCAN_SetAbitBaud）
canDLL.ZCAN_SetAbitBaud.restype = ctypes.c_uint
canDLL.ZCAN_SetAbitBaud.argtypes = (ctypes.c_void_p, ctypes.c_ulong, ctypes.c_ulong)

# 设置CANFD数据域波特率（手册3.17节 ZCAN_SetDbitBaud）
canDLL.ZCAN_SetDbitBaud.restype = ctypes.c_uint
canDLL.ZCAN_SetDbitBaud.argtypes = (ctypes.c_void_p, ctypes.c_ulong, ctypes.c_ulong)

# 设置CANFD标准（手册3.19节 ZCAN_SetCANFDStandard）
canDLL.ZCAN_SetCANFDStandard.restype = ctypes.c_uint
canDLL.ZCAN_SetCANFDStandard.argtypes = (ctypes.c_void_p, ctypes.c_ulong, ctypes.c_ulong)

# 初始化CAN通道（手册3.5节 ZCAN_InitCAN）
canDLL.ZCAN_InitCAN.restype = ctypes.c_void_p
canDLL.ZCAN_InitCAN.argtypes = (ctypes.c_void_p, ctypes.c_ulong, ctypes.c_void_p)

# 启动CAN通道（手册3.6节 ZCAN_StartCAN）
canDLL.ZCAN_StartCAN.restype = ctypes.c_uint
canDLL.ZCAN_StartCAN.argtypes = (ctypes.c_void_p,)

# 发送CAN报文（手册3.9节 ZCAN_Transmit）
canDLL.ZCAN_Transmit.restype = ctypes.c_uint
canDLL.ZCAN_Transmit.argtypes = (ctypes.c_void_p, ctypes.c_void_p, ctypes.c_ulong)

# 获取接收缓冲区帧数（手册3.11节 ZCAN_GetReceiveNum）
canDLL.ZCAN_GetReceiveNum.restype = ctypes.c_uint
canDLL.ZCAN_GetReceiveNum.argtypes = (ctypes.c_void_p, ctypes.c_ulong)

# 接收CAN报文（手册3.12节 ZCAN_Receive）
canDLL.ZCAN_Receive.restype = ctypes.c_uint
canDLL.ZCAN_Receive.argtypes = (ctypes.c_void_p, ctypes.c_void_p, ctypes.c_ulong, ctypes.c_long)

# 清除滤波配置（手册3.21节 ZCAN_ClearFilter）
canDLL.ZCAN_ClearFilter.restype = ctypes.c_uint
canDLL.ZCAN_ClearFilter.argtypes = (ctypes.c_void_p,)

# 设置滤波模式（手册3.22节 ZCAN_SetFilterMode）
canDLL.ZCAN_SetFilterMode.restype = ctypes.c_uint
canDLL.ZCAN_SetFilterMode.argtypes = (ctypes.c_void_p, ctypes.c_ulong)

# 设置滤波起始ID（手册3.23节 ZCAN_SetFilterStartID）
canDLL.ZCAN_SetFilterStartID.restype = ctypes.c_uint
canDLL.ZCAN_SetFilterStartID.argtypes = (ctypes.c_void_p, ctypes.c_ulong)

# 设置滤波结束ID（手册3.24节 ZCAN_SetFilterEndID）
canDLL.ZCAN_SetFilterEndID.restype = ctypes.c_uint
canDLL.ZCAN_SetFilterEndID.argtypes = (ctypes.c_void_p, ctypes.c_ulong)

# 生效滤波配置（手册3.25节 ZCAN_AckFilter）
canDLL.ZCAN_AckFilter.restype = ctypes.c_uint
canDLL.ZCAN_AckFilter.argtypes = (ctypes.c_void_p,)

# 复位CAN通道（手册3.7节 ZCAN_ResetCAN）
canDLL.ZCAN_ResetCAN.restype = ctypes.c_uint
canDLL.ZCAN_ResetCAN.argtypes = (ctypes.c_void_p,)

# 关闭设备（手册3.2节 ZCAN_CloseDevice）
canDLL.ZCAN_CloseDevice.restype = ctypes.c_uint
canDLL.ZCAN_CloseDevice.argtypes = (ctypes.c_void_p,)

# -------------------------- 核心功能类 --------------------------
class CANMessageSequence:
    """CAN消息序列管理类，支持CANopen CiA402协议，用于发送一组消息并等待每条消息的响应"""
    
    # CiA402 状态机常量
    STATE_NOT_READY_TO_SWITCH_ON = 0
    STATE_SWITCH_ON_DISABLED = 1
    STATE_READY_TO_SWITCH_ON = 2
    STATE_SWITCHED_ON = 3
    STATE_OPERATION_ENABLED = 4
    STATE_QUICK_STOP_ACTIVE = 5
    STATE_FAULT_REACTION_ACTIVE = 6
    STATE_FAULT = 7
    
    # CiA402 控制字命令
    CONTROL_SHUTDOWN = 0x06
    CONTROL_SWITCH_ON = 0x07
    CONTROL_ENABLE_OPERATION = 0x0F
    CONTROL_DISABLE_VOLTAGE = 0x00
    CONTROL_QUICK_STOP = 0x02
    CONTROL_DISABLE_OPERATION = 0x07
    CONTROL_FAULT_RESET = 0x80
    
    # CiA402 对象字典索引
    OD_CONTROL_WORD = 0x6040
    OD_STATUS_WORD = 0x6041
    OD_MODES_OF_OPERATION = 0x6060
    OD_MODES_OF_OPERATION_DISPLAY = 0x6061
    OD_POSITION_ACTUAL_VALUE = 0x6064
    OD_VELOCITY_ACTUAL_VALUE = 0x606C
    OD_TORQUE_ACTUAL_VALUE = 0x6077
    OD_TARGET_POSITION = 0x607A
    OD_TARGET_VELOCITY = 0x60FF
    OD_TARGET_TORQUE = 0x6071
    OD_MAX_PROFILE_VELOCITY = 0x607F
    OD_PROFILE_VELOCITY = 0x6081
    OD_PROFILE_ACCELERATION = 0x6083
    OD_PROFILE_DECELERATION = 0x6084
    OD_HOMING_METHOD = 0x6098
    OD_HOMING_SPEEDS = 0x6099
    OD_HOMING_ACCELERATION = 0x609A
    
    # 操作模式
    MODE_POSITION = 1
    MODE_VELOCITY = 3
    MODE_TORQUE = 4
    MODE_HOMING = 6
    MODE_INTERPOLATED_POSITION = 7
    MODE_CYCLIC_SYNC_POSITION = 8
    MODE_CYCLIC_SYNC_VELOCITY = 9
    MODE_CYCLIC_SYNC_TORQUE = 10
    
    def __init__(self, channel_handle, send_id=0x601, response_id=0x581, response_timeout=5):
        """
        初始化CAN消息序列管理器，支持CANopen CiA402协议
        
        Args:
            channel_handle: CAN通道句柄
            send_id: 发送消息的ID (默认0x601为SDO客户端)
            response_id: 期望接收响应的ID (默认0x581为SDO服务器)
            response_timeout: 等待响应的超时时间（秒）
        """
        self.channel_handle = channel_handle
        self.send_id = send_id
        self.response_id = response_id
        self.response_timeout = response_timeout
        self.message_sequences = []  # 存储所有消息序列
        self.current_state = self.STATE_SWITCH_ON_DISABLED
        self.current_mode = self.MODE_POSITION
    
    def add_sequence(self, name, messages):
        """
        添加一个消息序列
        
        Args:
            name: 序列名称
            messages: 消息列表，每个元素为包含数据的列表，例如 [0x2b, 0x40, 0x60, 0x00, 0x0f, 0x00, 0x00, 0x00]
        """
        self.message_sequences.append({
            'name': name,
            'messages': messages
        })
    
    def add_cia402_sequence(self, sequence_type, **kwargs):
        """
        添加标准的CiA402协议序列
        
        Args:
            sequence_type: 序列类型 ('init', 'shutdown', 'switch_on', 'enable', 'disable', 
                                 'quick_stop', 'fault_reset', 'set_mode', 'set_velocity', 
                                 'set_position', 'set_torque', 'homing')
            **kwargs: 序列参数，根据类型不同而不同
        """
        if sequence_type == 'init':
            self._add_init_sequence()
        elif sequence_type == 'shutdown':
            self._add_shutdown_sequence()
        elif sequence_type == 'switch_on':
            self._add_switch_on_sequence()
        elif sequence_type == 'enable':
            self._add_enable_operation_sequence()
        elif sequence_type == 'disable':
            self._add_disable_operation_sequence()
        elif sequence_type == 'quick_stop':
            self._add_quick_stop_sequence()
        elif sequence_type == 'fault_reset':
            self._add_fault_reset_sequence()
        elif sequence_type == 'set_mode':
            mode = kwargs.get('mode', self.MODE_POSITION)
            self._add_set_mode_sequence(mode)
        elif sequence_type == 'set_velocity':
            velocity = kwargs.get('velocity', 0)
            self._add_set_velocity_sequence(velocity)
        elif sequence_type == 'set_position':
            position = kwargs.get('position', 0)
            self._add_set_position_sequence(position)
        elif sequence_type == 'set_torque':
            torque = kwargs.get('torque', 0)
            self._add_set_torque_sequence(torque)
        elif sequence_type == 'homing':
            method = kwargs.get('method', 0)
            self._add_homing_sequence(method)
        else:
            print(f"❌ 未知的CiA402序列类型: {sequence_type}")
    
    def _add_init_sequence(self):
        """添加初始化序列"""
        self.message_sequences.append({
            'name': 'CiA402_初始化序列',
            'messages': [
                self._build_sdo_write(self.OD_CONTROL_WORD, 0, 0x00, 2),  # 禁用电压
                self._build_sdo_write(self.OD_MODES_OF_OPERATION, 0, self.MODE_POSITION, 1),  # 设置位置模式
            ]
        })
    
    def _add_shutdown_sequence(self):
        """添加关机序列"""
        self.message_sequences.append({
            'name': 'CiA402_关机序列',
            'messages': [
                self._build_sdo_write(self.OD_CONTROL_WORD, 0, self.CONTROL_SHUTDOWN, 2),
            ]
        })
    
    def _add_switch_on_sequence(self):
        """添加上电序列"""
        self.message_sequences.append({
            'name': 'CiA402_上电序列',
            'messages': [
                self._build_sdo_write(self.OD_CONTROL_WORD, 0, self.CONTROL_SWITCH_ON, 2),
            ]
        })
    
    def _add_enable_operation_sequence(self):
        """添加使能操作序列"""
        self.message_sequences.append({
            'name': 'CiA402_使能操作序列',
            'messages': [
                self._build_sdo_write(self.OD_CONTROL_WORD, 0, self.CONTROL_ENABLE_OPERATION, 2),
            ]
        })
    
    def _add_disable_operation_sequence(self):
        """添加禁用操作序列"""
        self.message_sequences.append({
            'name': 'CiA402_禁用操作序列',
            'messages': [
                self._build_sdo_write(self.OD_CONTROL_WORD, 0, self.CONTROL_DISABLE_OPERATION, 2),
            ]
        })
    
    def _add_quick_stop_sequence(self):
        """添加快速停止序列"""
        self.message_sequences.append({
            'name': 'CiA402_快速停止序列',
            'messages': [
                self._build_sdo_write(self.OD_CONTROL_WORD, 0, self.CONTROL_QUICK_STOP, 2),
            ]
        })
    
    def _add_fault_reset_sequence(self):
        """添加故障复位序列"""
        self.message_sequences.append({
            'name': 'CiA402_故障复位序列',
            'messages': [
                self._build_sdo_write(self.OD_CONTROL_WORD, 0, self.CONTROL_FAULT_RESET, 2),
            ]
        })
    
    def _add_set_mode_sequence(self, mode):
        """添加设置操作模式序列"""
        self.message_sequences.append({
            'name': f'CiA402_设置模式_{mode}',
            'messages': [
                self._build_sdo_write(self.OD_MODES_OF_OPERATION, 0, mode, 1),
            ]
        })
    
    def _add_set_velocity_sequence(self, velocity):
        """添加设置速度序列"""
        self.message_sequences.append({
            'name': f'CiA402_设置速度_{velocity}',
            'messages': [
                self._build_sdo_write(self.OD_TARGET_VELOCITY, 0, velocity, 4),
            ]
        })
    
    def _add_set_position_sequence(self, position):
        """添加设置位置序列"""
        self.message_sequences.append({
            'name': f'CiA402_设置位置_{position}',
            'messages': [
                self._build_sdo_write(self.OD_TARGET_POSITION, 0, position, 4),
            ]
        })
    
    def _add_set_torque_sequence(self, torque):
        """添加设置扭矩序列"""
        self.message_sequences.append({
            'name': f'CiA402_设置扭矩_{torque}',
            'messages': [
                self._build_sdo_write(self.OD_TARGET_TORQUE, 0, torque, 2),
            ]
        })
    
    def _add_homing_sequence(self, method):
        """添加回零序列"""
        self.message_sequences.append({
            'name': f'CiA402_回零操作_方法{method}',
            'messages': [
                self._build_sdo_write(self.OD_HOMING_METHOD, 0, method, 1),
                self._build_sdo_write(self.OD_CONTROL_WORD, 0, 0x1F, 2),  # 启动回零
            ]
        })
    
    def _build_sdo_write(self, index, subindex, data, data_size):
        """
        构建SDO写命令
        
        Args:
            index: 对象字典索引
            subindex: 子索引
            data: 要写入的数据
            data_size: 数据大小（字节数）
            
        Returns:
            list: CAN数据帧
        """
        # SDO写命令格式: [命令字, 索引低字节, 索引高字节, 子索引, 数据...]
        # 根据数据大小选择正确的命令字节（CANopen协议）
        if data_size == 1:
            cmd_byte = 0x2F  # 写1字节
            return [cmd_byte, index & 0xFF, (index >> 8) & 0xFF, subindex, data & 0xFF, 0, 0, 0]
        elif data_size == 2:
            cmd_byte = 0x2B  # 写2字节
            return [cmd_byte, index & 0xFF, (index >> 8) & 0xFF, subindex, 
                   data & 0xFF, (data >> 8) & 0xFF, 0, 0]
        elif data_size == 4:
            cmd_byte = 0x23  # 写4字节
            return [cmd_byte, index & 0xFF, (index >> 8) & 0xFF, subindex,
                   data & 0xFF, (data >> 8) & 0xFF, (data >> 16) & 0xFF, (data >> 24) & 0xFF]
        else:
            cmd_byte = 0x2B  # 默认使用2字节写命令
            return [cmd_byte, index & 0xFF, (index >> 8) & 0xFF, subindex, 0, 0, 0, 0]
    
    def _build_sdo_read(self, index, subindex):
        """
        构建SDO读命令
        
        Args:
            index: 对象字典索引
            subindex: 子索引
            
        Returns:
            list: CAN数据帧
        """
        # SDO读命令格式: [命令字, 索引低字节, 索引高字节, 子索引, 0, 0, 0, 0]
        return [0x40, index & 0xFF, (index >> 8) & 0xFF, subindex, 0, 0, 0, 0]
    
    def read_status_word(self):
        """读取状态字"""
        return self._send_and_receive_sdo(self.OD_STATUS_WORD, 0)
    
    def read_actual_position(self):
        """读取实际位置"""
        return self._send_and_receive_sdo(self.OD_POSITION_ACTUAL_VALUE, 0)
    
    def read_actual_velocity(self):
        """读取实际速度"""
        return self._send_and_receive_sdo(self.OD_VELOCITY_ACTUAL_VALUE, 0)
    
    def read_actual_torque(self):
        """读取实际扭矩"""
        return self._send_and_receive_sdo(self.OD_TORQUE_ACTUAL_VALUE, 0)
    
    def _send_and_receive_sdo(self, index, subindex):
        """
        发送SDO读命令并接收响应
        
        Args:
            index: 对象字典索引
            subindex: 子索引
            
        Returns:
            int: 读取到的数据，如果失败返回None
        """
        # 发送SDO读命令
        read_cmd = self._build_sdo_read(index, subindex)
        send_success = self._send_can_data(self.send_id, read_cmd)
        
        if not send_success:
            print(f"❌ SDO读命令发送失败: index=0x{index:04X}, subindex=0x{subindex:02X}")
            return None
        
        # 等待响应
        response_success, response_data = self._receive_can_data(self.response_id, self.response_timeout)
        
        if not response_success:
            print(f"❌ SDO读响应超时: index=0x{index:04X}, subindex=0x{subindex:02X}")
            return None
        
        # 解析响应数据 (SDO响应格式: [命令字, 索引低字节, 索引高字节, 子索引, 数据...])
        if len(response_data) >= 8 and (response_data[0] == 0x4B or response_data[0] == 0x43):  # SDO读响应（快速或标准）
            # 根据ZLAC8015D文档，状态字是16位数据，使用数据字节4-5（小端格式）
            data = (response_data[5] << 8) | response_data[4]
            print(f"✅ SDO响应解析成功: 命令字=0x{response_data[0]:02X}, 数据=0x{data:04X}")
            return data
        else:
            print(f"❌ SDO响应格式错误: {[hex(d) for d in response_data]}")
            print(f"   期望: 命令字=0x4B(快速)或0x43(标准)，实际=0x{response_data[0]:02X}")
        
        return None
    
    def get_state_name(self, state):
        """获取状态名称"""
        states = {
            self.STATE_NOT_READY_TO_SWITCH_ON: "Not ready to switch on",
            self.STATE_SWITCH_ON_DISABLED: "Switch on disabled",
            self.STATE_READY_TO_SWITCH_ON: "Ready to switch on",
            self.STATE_SWITCHED_ON: "Switched on",
            self.STATE_OPERATION_ENABLED: "Operation enabled",
            self.STATE_QUICK_STOP_ACTIVE: "Quick stop active",
            self.STATE_FAULT_REACTION_ACTIVE: "Fault reaction active",
            self.STATE_FAULT: "Fault"
        }
        return states.get(state, "Unknown state")
    
    def get_mode_name(self, mode):
        """获取模式名称"""
        modes = {
            self.MODE_POSITION: "Position mode",
            self.MODE_VELOCITY: "Velocity mode",
            self.MODE_TORQUE: "Torque mode",
            self.MODE_HOMING: "Homing mode",
            self.MODE_INTERPOLATED_POSITION: "Interpolated position mode",
            self.MODE_CYCLIC_SYNC_POSITION: "Cyclic sync position mode",
            self.MODE_CYCLIC_SYNC_VELOCITY: "Cyclic sync velocity mode",
            self.MODE_CYCLIC_SYNC_TORQUE: "Cyclic sync torque mode"
        }
        return modes.get(mode, "Unknown mode")
    
    def send_sequence(self, sequence_index):
        """
        发送指定索引的消息序列
        
        Args:
            sequence_index: 序列索引
            
        Returns:
            bool: 所有消息是否都发送成功并收到响应
        """
        if sequence_index >= len(self.message_sequences):
            print(f"❌ 序列索引 {sequence_index} 超出范围")
            return False
        
        sequence = self.message_sequences[sequence_index]
        print(f"\n🚀 开始发送序列: {sequence['name']}")
        
        for i, message_data in enumerate(sequence['messages']):
            print(f"\n📝 发送序列中的第 {i+1}/{len(sequence['messages'])} 条消息")
            
            # 发送消息
            send_success = self._send_can_data(self.send_id, message_data)
            
            if not send_success:
                print(f"❌ 发送序列中的第 {i+1} 条消息失败")
                return False
            
            # 等待响应
            print(f"⏳ 等待响应 from 0x{self.response_id:x}...")
            response_success, response_data = self._receive_can_data(self.response_id, self.response_timeout)
            
            if not response_success:
                print(f"❌ 等待第 {i+1} 条消息的响应超时")
                return False
            
            print(f"✅ 第 {i+1} 条消息处理完成")
        
        print(f"\n🎉 序列 {sequence['name']} 全部发送完成")
        return True
    
    def send_all_sequences(self):
        """
        发送所有消息序列
        
        Returns:
            bool: 所有序列是否都发送成功
        """
        for i in range(len(self.message_sequences)):
            if not self.send_sequence(i):
                print(f"❌ 序列 {i} 发送失败")
                return False
        return True
    
    def list_sequences(self):
        """列出所有已添加的消息序列"""
        print("\n📋 已配置的消息序列:")
        for i, sequence in enumerate(self.message_sequences):
            print(f"  {i}: {sequence['name']} ({len(sequence['messages'])} 条消息)")
            for j, msg in enumerate(sequence['messages']):
                print(f"    {j+1}. {[hex(d) for d in msg]}")
    
    def _send_can_data(self, send_id, data_list, is_ext_frame=False):
        """发送CAN数据（参考手册3.9节 ZCAN_Transmit）"""
        transmit_data = ZCAN_Transmit_Data()
        
        # 配置帧ID及标志位（手册2.3节 can_id格式）
        transmit_data.frame.can_id = send_id
        transmit_data.frame.eff = 1 if is_ext_frame else 0  # 扩展帧标志
        transmit_data.frame.rtr = 0  # 数据帧（非远程帧）
        transmit_data.frame.err = 0  # 正常帧
        
        # 配置数据长度及内容
        transmit_data.frame.can_dlc = len(data_list) if len(data_list) <= 8 else 8
        for i in range(transmit_data.frame.can_dlc):
            transmit_data.frame.data[i] = data_list[i]
        
        # 发送方式（0=正常发送，自动重发，手册2.5节）
        transmit_data.transmit_type = 0

        # 执行发送
        ret = canDLL.ZCAN_Transmit(self.channel_handle, ctypes.byref(transmit_data), 1)
        if ret == 1:
            print(f"\n✅ Send to 0x{send_id:x} success!")
            print(f"   DLC: {transmit_data.frame.can_dlc}, Data: {[hex(d) for d in data_list]}")
            return True
        else:
            print(f"\n❌ Send to 0x{send_id:x} failed!（返回值：{ret}）")
            return False

    def _receive_can_data(self, target_id, timeout=5, is_ext_frame=False):
        """接收CAN数据（参考手册3.11-3.12节）"""
        start_time = time.time()
        # 接收缓冲区（单次最大接收2500帧，参考手册例程）
        rcv_buf = (ZCAN_Receive_Data * 2500)()

        while time.time() - start_time < timeout:
            # 检查接收缓冲区帧数（0=CAN，1=CANFD，手册3.11节）
            count = canDLL.ZCAN_GetReceiveNum(self.channel_handle, TYPE_CAN)
            
            if count > 0:
                # 读取数据（wait_time=-1=无限等待，手册3.12节）
                read_len = canDLL.ZCAN_Receive(self.channel_handle, ctypes.byref(rcv_buf), count, -1)
                if read_len <= 0:
                    time.sleep(0.1)
                    continue

                # 解析接收数据
                print(f"\n✅ Received {read_len} frame(s):")
                for i in range(read_len):
                    recv_id = rcv_buf[i].frame.can_id
                    recv_eff = rcv_buf[i].frame.eff
                    # 匹配目标ID及帧类型
                    if recv_id == target_id and recv_eff == (1 if is_ext_frame else 0):
                        data = [rcv_buf[i].frame.data[j] for j in range(rcv_buf[i].frame.can_dlc)]
                        print(f"   Data: {[hex(d) for d in data]}")
                        return True, data
            time.sleep(0.1)  # 短延时降低CPU占用

        print(f"\n❌ Receive from 0x{target_id:x} timeout!（超时{timeout}秒）")
        return False, []# -------------------------- 速度控制类 --------------------------

class Motor_CTL(CANMessageSequence):
    """电机控制类，用于控制电机的速度模式"""
    
    # 控制方式常量
    SYNC_CONTROL = 0      # 同步控制
    ASYNC_CONTROL = 1     # 异步控制
    
    def __init__(self, channel_handle, send_id=0x601, response_id=0x581, response_timeout=5):
        """
        初始化电机控制器
        
        Args:
            channel_handle: CAN通道句柄
            send_id: 发送消息的ID
            response_id: 期望接收响应的ID
            response_timeout: 等待响应的超时时间（秒）
        """
        # 调用父类初始化
        super().__init__(channel_handle, send_id, response_id, response_timeout)
        
        self.control_mode = None  # 当前控制方式
        self.speed = 0            # 当前速度值
        
        # 初始化序列（根据控制方式不同而不同）
        self.sync_init_seq = [
            [0x2b, 0x0F, 0x20, 0x00, 0x01, 0x00, 0x00, 0x00],
            [0x2F, 0x60, 0x60, 0x00, 0x03, 0x00, 0x00, 0x00],
            [0x23, 0x83, 0x60, 0x01, 0x64, 0x00, 0x00, 0x00],
            [0x23, 0x83, 0x60, 0x02, 0x64, 0x00, 0x00, 0x00],
            [0x23, 0x84, 0x60, 0x01, 0x64, 0x00, 0x00, 0x00],
            [0x23, 0x84, 0x60, 0x02, 0x64, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x06, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x07, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x0F, 0x00, 0x00, 0x00]
        ]
        
        self.async_init_seq = [
            [0x2b, 0x0F, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00],
            [0x2F, 0x60, 0x60, 0x00, 0x03, 0x00, 0x00, 0x00],
            [0x23, 0x83, 0x60, 0x01, 0x64, 0x00, 0x00, 0x00],
            [0x23, 0x83, 0x60, 0x02, 0x64, 0x00, 0x00, 0x00],
            [0x23, 0x84, 0x60, 0x01, 0x64, 0x00, 0x00, 0x00],
            [0x23, 0x84, 0x60, 0x02, 0x64, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x06, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x07, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x0F, 0x00, 0x00, 0x00]
        ]
        
        # 速度控制序列模板
        self.speed_control_template = [0x23, 0xFF, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00]
    
    def set_control_mode(self, mode):
        """
        设置控制方式
        
        Args:
            mode: 控制方式 (SYNC_CONTROL 或 ASYNC_CONTROL)
        """
        if mode not in [self.SYNC_CONTROL, self.ASYNC_CONTROL]:
            print("❌ 无效的控制方式")
            return False
        
        self.control_mode = mode
        print(f"✅ 控制方式设置为: {'同步控制' if mode == self.SYNC_CONTROL else '异步控制'}")
        return True
    


    def _convert_speed_to_bytes(self, speed):
        """
        将速度值转换为字节序列
        
        Args:
            speed: 速度值 (-1000 到 1000)
            
        Returns:
            tuple: (byte_3, byte_4, byte_5, byte_6) 分别对应序列中的第4、5、6、7位
        """
        # 限制速度范围
        speed = max(-1000, min(1000, speed))
        
        if speed >= 0:
            # 正数处理
            byte_3 = speed & 0xFF           # 低8位
            byte_4 = (speed >> 8) & 0xFF    # 高8位
            byte_5 = 0x00                   # 负数标志位
            byte_6 = 0x00                   # 负数标志位
        else:
            # 负数处理
            speed = (1<<16) + speed
            byte_3 = speed & 0xFF      # 低8位
            byte_4 = (speed >> 8) & 0xFF    # 高8位
            byte_5 = 0xFF                   # 负数标志位
            byte_6 = 0xFF                   # 负数标志位
            
        return byte_3, byte_4, byte_5, byte_6
    
    def set_speed(self, speed, run_time_ms=None, left_speed=None, right_speed=None):
        """
        设置电机速度
        
        Args:
            speed: 速度值 (-1000 到 1000)，在同步控制模式下使用，或在异步控制模式下作为默认速度
            run_time_ms: 运行时间（毫秒），可选参数，如果提供则在指定时间后发送停止命令
            left_speed: 左电机速度值 (-1000 到 1000)，仅在异步控制模式下使用
            right_speed: 右电机速度值 (-1000 到 1000)，仅在异步控制模式下使用
            
        Returns:
            bool: 是否设置成功
        """
        if self.control_mode is None:
            print("❌ 请先设置控制方式")
            return False
        
        success = False
        if self.control_mode == self.SYNC_CONTROL:
            # 同步控制：同时控制两个电机，只需发送一条消息
            # 限制速度范围
            speed = max(-1000, min(1000, speed))
            self.speed = speed
            
            # 转换速度值为字节序列
            byte_3, byte_4, byte_5, byte_6 = self._convert_speed_to_bytes(speed)
            
            # 第4、5位（索引3、4）是左电机速度
            # 第6、7位（索引5、6）是右电机速度
            sync_speed_seq = self.speed_control_template[:]
            sync_speed_seq[3] = 0x03
            sync_speed_seq[4] = byte_3      # 左电机速度低8位
            sync_speed_seq[5] = byte_4      # 左电机速度高8位
            sync_speed_seq[6] = byte_3      # 右电机速度低8位
            sync_speed_seq[7] = byte_4      # 右电机速度高8位
            
            print(f"🚀 发送同步控制速度命令: {[hex(d) for d in sync_speed_seq]}")
            success = self._send_can_data(self.send_id, sync_speed_seq)
            
            if not success:
                print("❌ 同步控制速度设置失败")
                return False
            
            # 等待响应
            print(f"⏳ 等待响应 from 0x{self.response_id:x}...")
            response_success, _ = self._receive_can_data(self.response_id, self.response_timeout)
            
            if not response_success:
                print("❌ 同步控制响应超时")
                return False
                
            print(f"✅ 同步控制速度设置成功: {speed}")
        else:  # ASYNC_CONTROL
            # 异步控制：分别控制两个电机
            # 如果没有提供单独的左右电机速度，则使用speed参数作为默认速度
            if left_speed is None:
                left_speed = speed
            if right_speed is None:
                right_speed = speed
                
            # 限制速度范围
            left_speed = max(-1000, min(1000, left_speed))
            right_speed = max(-1000, min(1000, right_speed))
            self.speed = (left_speed, right_speed)  # 保存左右电机速度
            
            # 转换左右电机速度值为字节序列
            left_byte_3, left_byte_4, left_byte_5, left_byte_6 = self._convert_speed_to_bytes(left_speed)
            right_byte_3, right_byte_4, right_byte_5, right_byte_6 = self._convert_speed_to_bytes(right_speed)
            
            # 发送左电机速度控制序列
            left_speed_seq = self.speed_control_template[:]
            left_speed_seq[3] = 0x01        # 左电机ID
            left_speed_seq[4] = left_byte_3      # 速度低8位
            left_speed_seq[5] = left_byte_4      # 速度高8位
            left_speed_seq[6] = left_byte_5      # 负数标志位
            left_speed_seq[7] = left_byte_6      # 负数标志位
            
            print(f"🚀 发送左电机速度控制命令: {[hex(d) for d in left_speed_seq]}")
            success1 = self._send_can_data(self.send_id, left_speed_seq)
            
            if not success1:
                print("❌ 左电机速度设置失败")
                # 不立即返回，继续尝试右电机
            else:
                # 等待响应
                print(f"⏳ 等待左电机响应 from 0x{self.response_id:x}...")
                response_success1, _ = self._receive_can_data(self.response_id, self.response_timeout)
                
                if not response_success1:
                    print("❌ 左电机响应超时")
                    # 不立即返回，继续尝试右电机
            
            # 发送右电机速度控制序列
            right_speed_seq = self.speed_control_template[:]
            right_speed_seq[3] = 0x02       # 右电机ID
            right_speed_seq[4] = right_byte_3     # 速度低8位
            right_speed_seq[5] = right_byte_4     # 速度高8位
            right_speed_seq[6] = right_byte_5     # 负数标志位
            right_speed_seq[7] = right_byte_6     # 负数标志位
            
            print(f"🚀 发送右电机速度控制命令: {[hex(d) for d in right_speed_seq]}")
            success2 = self._send_can_data(self.send_id, right_speed_seq)
            
            if not success2:
                print("❌ 右电机速度设置失败")
                # 不立即返回，继续处理运行时间
            else:
                # 等待响应
                print(f"⏳ 等待右电机响应 from 0x{self.response_id:x}...")
                response_success2, _ = self._receive_can_data(self.response_id, self.response_timeout)
                
                if not response_success2:
                    print("❌ 右电机响应超时")
                    # 不立即返回，继续处理运行时间
                
            # 只要至少有一个电机设置成功，就认为成功
            success = success1 or success2
            if success:
                print(f"✅ 异步控制速度设置成功: 左电机 {left_speed}, 右电机 {right_speed}")
            else:
                print("❌ 异步控制速度设置完全失败")
        
        # 如果提供了运行时间参数，则等待指定时间后发送停止命令
        # 注意：无论同步还是异步控制模式，只要提供了运行时间参数，就应该发送停止命令
        if run_time_ms is not None and success:
            print(f"⏳ 电机将运行 {run_time_ms} 毫秒...")
            time.sleep(run_time_ms / 1000.0)  # 转换为秒
            
            # 发送停止命令
            stop_command = [0x2b, 0x40, 0x60, 0x00, 0x02, 0x00, 0x00, 0x00]
            print(f"🛑 发送停止命令: {[hex(d) for d in stop_command]}")
            stop_success = self._send_can_data(self.send_id, stop_command)
            
            if not stop_success:
                print("❌ 停止命令发送失败")
                return False
            
            # 等待响应
            print(f"⏳ 等待停止命令响应 from 0x{self.response_id:x}...")
            stop_response_success, _ = self._receive_can_data(self.response_id, self.response_timeout)
            
            if not stop_response_success:
                print("❌ 停止命令响应超时")
                return False
                
            print("✅ 电机已停止")
        
        return success

    def initialize_motor(self):
        """
        初始化电机，根据当前控制方式发送相应的初始化序列
        
        Returns:
            bool: 是否初始化成功
        """
        if self.control_mode is None:
            print("❌ 请先设置控制方式")
            return False
        
        print(f"\n🚀 开始{'同步' if self.control_mode == self.SYNC_CONTROL else '异步'}控制初始化...")
        
        # 选择对应的初始化序列
        init_seq = self.sync_init_seq if self.control_mode == self.SYNC_CONTROL else self.async_init_seq
        
        # 发送初始化序列
        for i, message_data in enumerate(init_seq):
            print(f"\n📝 发送初始化序列中的第 {i+1}/{len(init_seq)} 条消息")
            
            # 发送消息
            send_success = self._send_can_data(self.send_id, message_data)
            
            if not send_success:
                print(f"❌ 发送初始化序列中的第 {i+1} 条消息失败")
                return False
            
            # 等待响应
            print(f"⏳ 等待响应 from 0x{self.response_id:x}...")
            response_success, response_data = self._receive_can_data(self.response_id, self.response_timeout)
            
            if not response_success:
                print(f"❌ 等待第 {i+1} 条消息的响应超时")
                return False
            
            print(f"✅ 第 {i+1} 条初始化消息处理完成")
        
        print(f"\n🎉 {'同步' if self.control_mode == self.SYNC_CONTROL else '异步'}控制初始化完成")
# -------------------------- 位置控制类 --------------------------
class Motor_CTL_Pos(CANMessageSequence):
    """电机位置控制类，用于控制电机的位置模式（同步控制）"""
    
    # 位置模式常量
    RELATIVE_POSITION = 0  # 相对位置
    ABSOLUTE_POSITION = 1  # 绝对位置
    
    def __init__(self, channel_handle, send_id=0x601, response_id=0x581, response_timeout=5):
        """
        初始化电机位置控制器
        
        Args:
            channel_handle: CAN通道句柄
            send_id: 发送消息的ID
            response_id: 期望接收响应的ID
            response_timeout: 等待响应的超时时间（秒）
        """
        super().__init__(channel_handle, send_id, response_id, response_timeout)
        self.position_mode = None  # 当前位置模式
        self.max_velocity = 60    # 默认最大速度60r/min
        
        # 初始化序列将在initialize_motor中动态构建
        self.init_seq = []
        
        # 位置控制启动序列
        self.relative_start_seq = [
            [0x2B, 0x40, 0x60, 0x00, 0x4F, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x5F, 0x00, 0x00, 0x00]
        ]
        
        self.absolute_start_seq = [
            [0x2B, 0x40, 0x60, 0x00, 0x0F, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x1F, 0x00, 0x00, 0x00]
        ]
        
        # 位置控制序列模板
        self.position_control_template = [0x23, 0x7a, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00]
    
    def set_max_velocity(self, velocity):
        """
        设置最大速度
        
        Args:
            velocity: 最大速度值 (r/min)
        """
        self.max_velocity = velocity
        print(f"✅ 最大速度设置为: {velocity} r/min")
        return True

    def _build_init_sequence(self):
        """构建初始化序列，包含动态的最大速度设置"""
        self.init_seq = [
            [0x2F, 0x60, 0x60, 0x00, 0x01, 0x00, 0x00, 0x00],
            [0x23, 0x83, 0x60, 0x01, 0x64, 0x00, 0x00, 0x00],
            [0x23, 0x83, 0x60, 0x02, 0x64, 0x00, 0x00, 0x00],
            [0x23, 0x84, 0x60, 0x01, 0x64, 0x00, 0x00, 0x00],
            [0x23, 0x84, 0x60, 0x02, 0x64, 0x00, 0x00, 0x00],
            [0x23, 0x81, 0x60, 0x01, self.max_velocity, 0x00, 0x00, 0x00],
            [0x23, 0x81, 0x60, 0x02, self.max_velocity, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x06, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x07, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x0F, 0x00, 0x00, 0x00]
        ]
    
    def set_position_mode(self, mode):
        """
        设置位置模式
        
        Args:
            mode: 位置模式 (RELATIVE_POSITION 或 ABSOLUTE_POSITION)
        """
        if mode not in [self.RELATIVE_POSITION, self.ABSOLUTE_POSITION]:
            print("❌ 无效的位置模式")
            return False
        
        self.position_mode = mode
        print(f"✅ 位置模式设置为: {'相对位置' if mode == self.RELATIVE_POSITION else '绝对位置'}")
        return True
    


    def _convert_position_to_bytes(self, position):
        """
        将位置值转换为字节序列
        
        Args:
            position: 位置值 (有符号整数)
            
        Returns:
            tuple: (pos_l, pos_h, sign_flag) 分别对应位置的低8位、高8位和符号标志
        """
        if position >= 0:
            # 正数处理
            pos_l = position & 0xFF           # 低8位
            pos_h = (position >> 8) & 0xFF    # 高8位
            sign_flag = 0x00                  # 正数标志位
        else:
            # 负数处理（补码方式）
            position = (1 << 16) + position   # 转换为补码
            pos_l = position & 0xFF           # 低8位
            pos_h = (position >> 8) & 0xFF    # 高8位
            sign_flag = 0xFF                  # 负数标志位
            
        return pos_l, pos_h, sign_flag
    
    def set_position(self, position_mode, left_position, right_position):
        """
        设置电机位置
        
        Args:
            position_mode: 位置模式 (RELATIVE_POSITION 或 ABSOLUTE_POSITION)
            left_position: 左电机目标位置
            right_position: 右电机目标位置
            
        Returns:
            bool: 是否设置成功
        """
        if position_mode not in [self.RELATIVE_POSITION, self.ABSOLUTE_POSITION]:
            print("❌ 无效的位置模式")
            return False
        
        # 设置位置模式
        self.position_mode = position_mode
        
        success = True
        
        # 设置左电机位置
        left_success = self._set_single_motor_position(0x01, left_position)
        if not left_success:
            print("❌ 左电机位置设置失败")
            success = False
        
        # 设置右电机位置
        right_success = self._set_single_motor_position(0x02, right_position)
        if not right_success:
            print("❌ 右电机位置设置失败")
            success = False
        
        if success:
            # 发送启动序列
            start_seq = self.relative_start_seq if position_mode == self.RELATIVE_POSITION else self.absolute_start_seq
            
            print(f"\n🚀 发送{'相对' if position_mode == self.RELATIVE_POSITION else '绝对'}位置启动序列")
            for i, start_cmd in enumerate(start_seq):
                print(f"📝 发送启动命令 {i+1}/{len(start_seq)}: {[hex(d) for d in start_cmd]}")
                start_success = self._send_can_data(self.send_id, start_cmd)
                
                if not start_success:
                    print(f"❌ 启动命令 {i+1} 发送失败")
                    return False
                
                # 等待响应
                print(f"⏳ 等待启动命令响应 from 0x{self.response_id:x}...")
                response_success, _ = self._receive_can_data(self.response_id, self.response_timeout)
                
                if not response_success:
                    print(f"❌ 启动命令 {i+1} 响应超时")
                    return False
            
            print(f"✅ {'相对' if position_mode == self.RELATIVE_POSITION else '绝对'}位置控制启动成功")
            print(f"✅ 位置设置完成: 左电机 {left_position}, 右电机 {right_position}")
        
        return success
    
    def _set_single_motor_position(self, motor_id, position):
        """
        设置单个电机的位置
        
        Args:
            motor_id: 电机ID (0x01=左电机, 0x02=右电机)
            position: 目标位置
            
        Returns:
            bool: 是否设置成功
        """
        # 转换位置值为字节序列
        pos_l, pos_h, sign_flag = self._convert_position_to_bytes(position)
        
        # 构建位置控制序列
        position_seq = self.position_control_template[:]
        position_seq[3] = motor_id        # 电机ID
        position_seq[4] = pos_l           
        position_seq[5] = pos_h           
        position_seq[6] = sign_flag        
        position_seq[7] = sign_flag       # 符号标志位
        
        motor_name = "左电机" if motor_id == 0x01 else "右电机"
        
        # 重试机制：最多尝试3次
        max_retries = 3
        for attempt in range(1, max_retries + 1):
            print(f"🔄 第{attempt}次尝试设置{motor_name}位置...")
            
            print(f"🚀 发送{motor_name}位置控制命令: {[hex(d) for d in position_seq]}")
            success = self._send_can_data(self.send_id, position_seq)
            
            if not success:
                print(f"❌ 第{attempt}次发送{motor_name}位置控制命令失败")
                if attempt < max_retries:
                    print(f"⏳ 等待1秒后重试...")
                    time.sleep(1)
                    continue
                else:
                    print(f"❌ {motor_name}位置设置失败，已达到最大重试次数{max_retries}")
                    return False
            
            # 等待响应并验证响应数据
            print(f"⏳ 等待{motor_name}响应 from 0x{self.response_id:x}...")
            response_success, response_data = self._receive_can_data(self.response_id, self.response_timeout)
            
            if not response_success:
                print(f"❌ 第{attempt}次{motor_name}响应超时")
                if attempt < max_retries:
                    print(f"⏳ 等待1秒后重试...")
                    time.sleep(1)
                    continue
                else:
                    print(f"❌ {motor_name}响应超时，已达到最大重试次数{max_retries}")
                    return False
            
            # 验证响应数据格式
            # 左电机期望: [0x60, 0x7a, 0x60, 0x01, 0x00, 0x00, 0x00, 0x00]
            # 右电机期望: [0x60, 0x7a, 0x60, 0x02, 0x00, 0x00, 0x00, 0x00]
            expected_response = [0x60, 0x7a, 0x60, motor_id, 0x00, 0x00, 0x00, 0x00]
            
            if len(response_data) >= 8:
                # 检查关键字节：索引0、1、2、3应该匹配期望值
                if (response_data[0] == expected_response[0] and  # 0x60
                    response_data[1] == expected_response[1] and  # 0x7a
                    response_data[2] == expected_response[2] and  # 0x60
                    response_data[3] == expected_response[3]):    # motor_id
                    print(f"✅ {motor_name}位置设置成功: {position}")
                    print(f"✅ 从机响应正确: {[hex(d) for d in response_data]}")
                    return True
                else:
                    print(f"❌ 第{attempt}次{motor_name}响应数据格式错误")
                    print(f"   期望: {[hex(d) for d in expected_response]}")
                    print(f"   实际: {[hex(d) for d in response_data]}")
                    if attempt < max_retries:
                        print(f"⏳ 等待1秒后重试...")
                        time.sleep(1)
                        continue
                    else:
                        print(f"❌ {motor_name}响应数据格式错误，已达到最大重试次数{max_retries}")
                        print("⚠️  请检查电机配置和通信状态")
                        return False
            else:
                print(f"❌ 第{attempt}次{motor_name}响应数据长度不足: {len(response_data)}字节")
                print(f"   实际响应: {[hex(d) for d in response_data]}")
                if attempt < max_retries:
                    print(f"⏳ 等待1秒后重试...")
                    time.sleep(1)
                    continue
                else:
                    print(f"❌ {motor_name}响应数据长度不足，已达到最大重试次数{max_retries}")
                    return False
        
        # 理论上不会执行到这里
        return False
    
    def initialize_motor(self):
        """
        初始化电机，发送初始化序列
        
        Returns:
            bool: 是否初始化成功
        """
        print(f"\n🚀 开始位置控制初始化（最大速度: {self.max_velocity} r/min）...")
        
        # 构建初始化序列
        self._build_init_sequence()
        
        # 发送初始化序列
        for i, message_data in enumerate(self.init_seq):
            print(f"\n📝 发送初始化序列中的第 {i+1}/{len(self.init_seq)} 条消息")
            
            # 发送消息
            send_success = self._send_can_data(self.send_id, message_data)
            
            if not send_success:
                print(f"❌ 发送初始化序列中的第 {i+1} 条消息失败")
                return False
            
            # 等待响应
            print(f"⏳ 等待响应 from 0x{self.response_id:x}...")
            response_success, response_data = self._receive_can_data(self.response_id, self.response_timeout)
            
            if not response_success:
                print(f"❌ 等待第 {i+1} 条消息的响应超时")
                return False
            
            print(f"✅ 第 {i+1} 条初始化消息处理完成")
        
        print("\n🎉 位置控制初始化完成")
        return True
# -------------------------- 力矩控制类 --------------------------
class Motor_CTL_Ser(CANMessageSequence):
    """电机转矩控制类，用于控制电机的转矩模式"""
    
    # 控制方式常量
    SYNC_CONTROL = 0      # 同步控制
    ASYNC_CONTROL = 1     # 异步控制
    
    def __init__(self, channel_handle, send_id=0x601, response_id=0x581, response_timeout=5):
        """
        初始化电机转矩控制器
        
        Args:
            channel_handle: CAN通道句柄
            send_id: 发送消息的ID
            response_id: 期望接收响应的ID
            response_timeout: 等待响应的超时时间（秒）
        """
        super().__init__(channel_handle, send_id, response_id, response_timeout)
        self.control_mode = None  # 当前控制方式
        self.torque = 0           # 当前转矩值
        
        # 初始化序列（根据控制方式不同而不同）
        self.sync_init_seq = [
            [0x2b, 0x0F, 0x20, 0x00, 0x01, 0x00, 0x00, 0x00],
            [0x2F, 0x60, 0x60, 0x00, 0x04, 0x00, 0x00, 0x00],
            [0x23, 0x87, 0x60, 0x01, 0x64, 0x00, 0x00, 0x00],
            [0x23, 0x87, 0x60, 0x02, 0x64, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x06, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x07, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x0F, 0x00, 0x00, 0x00]
        ]
        
        self.async_init_seq = [
            [0x2b, 0x0F, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00],
            [0x2F, 0x60, 0x60, 0x00, 0x04, 0x00, 0x00, 0x00],
            [0x23, 0x87, 0x60, 0x01, 0x64, 0x00, 0x00, 0x00],
            [0x23, 0x87, 0x60, 0x02, 0x64, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x06, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x07, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x0F, 0x00, 0x00, 0x00]
        ]
        
        # 转矩控制序列模板
        self.torque_control_template = [0x23, 0x71, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00]
    
    def set_control_mode(self, mode):
        """
        设置控制方式
        
        Args:
            mode: 控制方式 (SYNC_CONTROL 或 ASYNC_CONTROL)
        """
        if mode not in [self.SYNC_CONTROL, self.ASYNC_CONTROL]:
            print("❌ 无效的控制方式")
            return False
        
        self.control_mode = mode
        print(f"✅ 控制方式设置为: {'同步控制' if mode == self.SYNC_CONTROL else '异步控制'}")
        return True
    
    def _send_can_data(self, send_id, data_list, is_ext_frame=False):
        """发送CAN数据（参考手册3.9节 ZCAN_Transmit）"""
        transmit_data = ZCAN_Transmit_Data()
        
        # 配置帧ID及标志位（手册2.3节 can_id格式）
        transmit_data.frame.can_id = send_id
        transmit_data.frame.eff = 1 if is_ext_frame else 0  # 扩展帧标志
        transmit_data.frame.rtr = 0  # 数据帧（非远程帧）
        transmit_data.frame.err = 0  # 正常帧
        
        # 配置数据长度及内容
        transmit_data.frame.can_dlc = len(data_list) if len(data_list) <= 8 else 8
        for i in range(transmit_data.frame.can_dlc):
            transmit_data.frame.data[i] = data_list[i]
        
        # 发送方式（0=正常发送，自动重发，手册2.5节）
        transmit_data.transmit_type = 0

        # 执行发送
        ret = canDLL.ZCAN_Transmit(self.channel_handle, ctypes.byref(transmit_data), 1)
        if ret == 1:
            print(f"\n✅ Send to 0x{send_id:x} success!")
            print(f"   DLC: {transmit_data.frame.can_dlc}, Data: {[hex(d) for d in data_list]}")
            return True
        else:
            print(f"\n❌ Send to 0x{send_id:x} failed!（返回值：{ret}）")
            return False

    def _convert_torque_to_bytes(self, torque):
        """
        将转矩值转换为字节序列
        
        Args:
            torque: 转矩值 (-32768 到 32767)
            
        Returns:
            tuple: (byte_low, byte_high) 分别对应转矩数据的低8位和高8位
        """
        # 限制转矩范围
        torque = max(-32768, min(32767, torque))
        
        if torque >= 0:
            # 正数处理
            byte_low = torque & 0xFF           # 低8位
            byte_high = (torque >> 8) & 0xFF   # 高8位
        else:
            # 负数处理（补码方式）
            torque = (1<<16) + torque
            byte_low = torque & 0xFF      # 低8位
            byte_high = (torque >> 8) & 0xFF    # 高8位
            
        return byte_low, byte_high
    
    def set_torque(self, control_mode, torque, left_torque=None, right_torque=None):
        """
        设置电机转矩
        
        Args:
            control_mode: 控制方式 (SYNC_CONTROL 或 ASYNC_CONTROL)
            torque: 转矩值，在同步控制模式下使用，或在异步控制模式下作为默认转矩
            left_torque: 左电机转矩值，仅在异步控制模式下使用
            right_torque: 右电机转矩值，仅在异步控制模式下使用
            
        Returns:
            bool: 是否设置成功
        """
        # 设置控制方式
        if not self.set_control_mode(control_mode):
            return False
        
        success = False
        if self.control_mode == self.SYNC_CONTROL:
            # 同步控制：同时控制两个电机，使用一条消息
            # 限制转矩范围
            torque = max(-32768, min(32767, torque))
            self.torque = torque
            
            # 转换转矩值为字节序列
            left_byte_low, left_byte_high = self._convert_torque_to_bytes(torque)
            right_byte_low, right_byte_high = self._convert_torque_to_bytes(torque)
            
            # 同步控制转矩序列 [23 71 60 03 ser_left_l ser_left_h ser_right_l ser_right_h]
            sync_torque_seq = [0x23, 0x71, 0x60, 0x03, 
                              left_byte_low, left_byte_high, 
                              right_byte_low, right_byte_high]
            
            print(f"🚀 发送同步控制转矩命令: {[hex(d) for d in sync_torque_seq]}")
            success = self._send_can_data(self.send_id, sync_torque_seq)
            
            if not success:
                print("❌ 同步控制转矩设置失败")
                return False
            
            # 等待响应
            print(f"⏳ 等待响应 from 0x{self.response_id:x}...")
            response_success, _ = self._receive_can_data(self.response_id, self.response_timeout)
            
            if not response_success:
                print("❌ 同步控制响应超时")
                return False
                
            print(f"✅ 同步控制转矩设置成功: {torque}")
        else:  # ASYNC_CONTROL
            # 异步控制：分别控制两个电机
            # 如果没有提供单独的左右电机转矩，则使用torque参数作为默认转矩
            if left_torque is None:
                left_torque = torque
            if right_torque is None:
                right_torque = torque
                
            # 限制转矩范围
            left_torque = max(-32768, min(32767, left_torque))
            right_torque = max(-32768, min(32767, right_torque))
            self.torque = (left_torque, right_torque)  # 保存左右电机转矩
            
            # 发送左电机转矩控制序列 [2B 71 60 01 ser_l ser_h 00 00]
            left_byte_low, left_byte_high = self._convert_torque_to_bytes(left_torque)
            # 如果是负数，第6和第7位设为FF
            left_byte_6 = 0xFF if left_torque < 0 else 0x00
            left_byte_7 = 0xFF if left_torque < 0 else 0x00
            left_torque_seq = [0x2B, 0x71, 0x60, 0x01, 
                              left_byte_low, left_byte_high, left_byte_6, left_byte_7]
            
            print(f"🚀 发送左电机转矩控制命令: {[hex(d) for d in left_torque_seq]}")
            success1 = self._send_can_data(self.send_id, left_torque_seq)
            
            if not success1:
                print("❌ 左电机转矩设置失败")
                # 不立即返回，继续尝试右电机
            else:
                # 等待响应
                print(f"⏳ 等待左电机响应 from 0x{self.response_id:x}...")
                response_success1, _ = self._receive_can_data(self.response_id, self.response_timeout)
                
                if not response_success1:
                    print("❌ 左电机响应超时")
                    # 不立即返回，继续尝试右电机
            
            # 发送右电机转矩控制序列 [2B 71 60 02 ser_l ser_h 00 00]
            right_byte_low, right_byte_high = self._convert_torque_to_bytes(right_torque)
            # 如果是负数，第6和第7位设为FF
            right_byte_6 = 0xFF if right_torque < 0 else 0x00
            right_byte_7 = 0xFF if right_torque < 0 else 0x00
            right_torque_seq = [0x2B, 0x71, 0x60, 0x02, 
                               right_byte_low, right_byte_high, right_byte_6, right_byte_7]
            
            print(f"🚀 发送右电机转矩控制命令: {[hex(d) for d in right_torque_seq]}")
            success2 = self._send_can_data(self.send_id, right_torque_seq)
            
            if not success2:
                print("❌ 右电机转矩设置失败")
                # 不立即返回，继续处理
            else:
                # 等待响应
                print(f"⏳ 等待右电机响应 from 0x{self.response_id:x}...")
                response_success2, _ = self._receive_can_data(self.response_id, self.response_timeout)
                
                if not response_success2:
                    print("❌ 右电机响应超时")
                    # 不立即返回，继续处理
                
            # 只要至少有一个电机设置成功，就认为成功
            success = success1 or success2
            if success:
                print(f"✅ 异步控制转矩设置成功: 左电机 {left_torque}, 右电机 {right_torque}")
            else:
                print("❌ 异步控制转矩设置完全失败")
        
        return success

    def initialize_motor(self):
        """
        初始化电机，根据当前控制方式发送相应的初始化序列
        
        Returns:
            bool: 是否初始化成功
        """
        if self.control_mode is None:
            print("❌ 请先设置控制方式")
            return False
        
        print(f"\n🚀 开始{'同步' if self.control_mode == self.SYNC_CONTROL else '异步'}控制初始化...")
        
        # 选择对应的初始化序列
        init_seq = self.sync_init_seq if self.control_mode == self.SYNC_CONTROL else self.async_init_seq
        
        # 发送初始化序列
        for i, message_data in enumerate(init_seq):
            print(f"\n📝 发送初始化序列中的第 {i+1}/{len(init_seq)} 条消息")
            
            # 发送消息
            send_success = self._send_can_data(self.send_id, message_data)
            
            if not send_success:
                print(f"❌ 发送初始化序列中的第 {i+1} 条消息失败")
                return False
            
            # 等待响应
            print(f"⏳ 等待响应 from 0x{self.response_id:x}...")
            response_success, response_data = self._receive_can_data(self.response_id, self.response_timeout)
            
            if not response_success:
                print(f"❌ 等待第 {i+1} 条消息的响应超时")
                return False
            
            print(f"✅ 第 {i+1} 条初始化消息处理完成")
        
        print(f"\n🎉 {'同步' if self.control_mode == self.SYNC_CONTROL else '异步'}控制初始化完成")

def init_can_device():
    """初始化CAN设备（通道0：收发0x601/0x581，参考手册5.1节流程）"""
    # 1. 打开设备（设备类型=41，索引=0，保留=0）
    m_dev = canDLL.ZCAN_OpenDevice(VCI_USBCAN2, 0, 0)
    if m_dev == INVALID_DEVICE_HANDLE:
        print("❌ Open Device failed!（检查DLL路径/设备类型/硬件连接）")
        return None, None
    print(f"✅ Open Device OK, device handle:0x{ctypes.c_uint64(m_dev).value:x}")

    # 2. 配置通道0波特率（500Kbps，参考手册3.16/3.17节）
    # 仲裁域波特率（CAN/CANFD通用）
    ret = canDLL.ZCAN_SetAbitBaud(m_dev, 0, 500000)
    if ret != STATUS_OK:
        print("❌ Set CAN0 abit:500K failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    print("✅ Set CAN0 abit:500K OK!")

    # 数据域波特率（CANFD专用，CAN可忽略但需调用）
    ret = canDLL.ZCAN_SetDbitBaud(m_dev, 0, 500000)
    if ret != STATUS_OK:
        print("❌ Set CAN0 dbit:500K failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    print("✅ Set CAN0 dbit:500K OK!")

    # 3. 设置CANFD标准（ISO模式，参考手册3.19节）
    ret = canDLL.ZCAN_SetCANFDStandard(m_dev, 0, 0)  # 0=ISO，1=BOSCH
    if ret != STATUS_OK:
        print("❌ Set CAN0 ISO mode failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    print("✅ Set CAN0 ISO mode OK!")

    # 4. 初始化通道0（配置为CANFD设备，参考手册2.2节）
    init_config = ZCAN_CHANNEL_INIT_CONFIG()
    init_config.can_type = TYPE_CANFD  # 按实际帧类型选择（TYPE_CAN/TYPE_CANFD）
    
    # 配置CANFD参数（基础滤波：接收所有帧）
    init_config.config.canfd.acc_code = 0x00000000    # 验收码设0
    init_config.config.canfd.acc_mask = 0xFFFFFFFF    # 屏蔽码全1=接收所有帧
    init_config.config.canfd.filter = 0               # 双滤波模式
    init_config.config.canfd.mode = 0                 # 正常工作模式
    init_config.config.canfd.brp = 0                  # 预分频设0
    init_config.config.canfd.reserved = 0             # 保留设0

    # 执行初始化
    dev_ch1 = canDLL.ZCAN_InitCAN(m_dev, 0, ctypes.byref(init_config))  # 通道0
    if dev_ch1 == INVALID_CHANNEL_HANDLE:
        print("❌ Init CAN0 failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    print("✅ Init CAN0 OK!")

    # 5. 配置滤波（仅接收0x581，手册3.21-3.25节，需在Start前配置）
    # 清除旧滤波
    ret = canDLL.ZCAN_ClearFilter(dev_ch1)
    if ret != STATUS_OK:
        print("❌ Clear CAN0 filter failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    # 设置滤波模式（0=标准帧，1=扩展帧，按0x581实际类型选择）
    ret = canDLL.ZCAN_SetFilterMode(dev_ch1, FILTER_MODE_STD)
    if ret != STATUS_OK:
        print("❌ Set CAN0 filter mode failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    # 设置滤波ID范围（0x581-0x581，仅接收该ID）
    ret = canDLL.ZCAN_SetFilterStartID(dev_ch1, 0x581)
    if ret != STATUS_OK:
        print("❌ Set CAN0 filter start ID failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    ret = canDLL.ZCAN_SetFilterEndID(dev_ch1, 0x581)
    if ret != STATUS_OK:
        print("❌ Set CAN0 filter end ID failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    # 生效滤波
    ret = canDLL.ZCAN_AckFilter(dev_ch1)
    if ret != STATUS_OK:
        print("❌ Ack CAN0 filter failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    print("✅ Configure CAN0 filter OK!（仅接收0x581）")

    # 6. 启动通道0
    ret = canDLL.ZCAN_StartCAN(dev_ch1)
    if ret != STATUS_OK:
        print("❌ Start CAN0 failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    print("✅ Start CAN0 OK!")

    return m_dev, dev_ch1

def close_can_device(m_dev, channel_handle):
    """关闭CAN设备（参考手册3.2/3.7节）"""
    # 复位通道
    if channel_handle:
        ret = canDLL.ZCAN_ResetCAN(channel_handle)
        print(f"\n{'✅ Close Channel OK!' if ret == STATUS_OK else '❌ Close Channel failed!'}")
    
    # 关闭设备
    if m_dev:
        ret = canDLL.ZCAN_CloseDevice(m_dev)
        print(f"{'✅ Close Device OK!' if ret == STATUS_OK else '❌ Close Device failed!'}")

# -------------------------- 主函数 --------------------------
if __name__ == "__main__":
    # 初始化设备（通道0）
    m_dev, dev_ch1 = init_can_device()
    if not m_dev or not dev_ch1:
        exit(1)

    try:
        # 等待设备稳定
        print("\n⌛ Waiting 2 seconds for device stabilization...")
        time.sleep(2)

        # 创建消息序列管理器
        msg_seq = CANMessageSequence(dev_ch1, send_id=0x601, response_id=0x581, response_timeout=5)
        
        # Init_seq = [
        #     [0x2b, 0x0F, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00],
        #     [0x2F, 0x60, 0x60, 0x00, 0x03, 0x00, 0x00, 0x00],
        #     [0x23, 0x83, 0x60, 0x01, 0x64, 0x00, 0x00, 0x00],
        #     [0x23, 0x83, 0x60, 0x02, 0x64, 0x00, 0x00, 0x00],
        #     [0x23, 0x84, 0x60, 0x01, 0x64, 0x00, 0x00, 0x00],
        #     [0x23, 0x84, 0x60, 0x02, 0x64, 0x00, 0x00, 0x00],
        #     [0x2B, 0x40, 0x60, 0x00, 0x06, 0x00, 0x00, 0x00],
        #     [0x2B, 0x40, 0x60, 0x00, 0x07, 0x00, 0x00, 0x00],
        #     [0x2B, 0x40, 0x60, 0x00, 0x0F, 0x00, 0x00, 0x00]
        # ]
        # msg_seq.add_sequence("初始化序列", Init_seq)
        
        # # 示例序列2
        # L_Motor_seq = [
        #     [0x23, 0xFF, 0x60, 0x01, 0x64, 0x00, 0x00, 0x00]
        # ]
        # msg_seq.add_sequence("左电机配置序列", L_Motor_seq)
        
        # R_Motor_seq = [
        #     [0x23, 0xFF, 0x60, 0x02, 0x9C, 0xFF, 0xFF, 0xFF]
        # ]
        # msg_seq.add_sequence("右电机配置序列", R_Motor_seq)
        # # Stop_seq = [
        # #     [0x2b, 0x40, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00]
        # # ]
        # # msg_seq.add_sequence("急停配置序列", Stop_seq)
        # # # 显示所有序列
        # # msg_seq.list_sequences()
        
        # # 发送所有序列
        # print("\n🚀 开始发送所有消息序列...")
        # success = msg_seq.send_all_sequences()
        
        # if success:
        #     print("\n🎉 所有消息序列发送完成!")
        # else:
        #     print("\n❌ 消息序列发送过程中出现错误!")

        # # 测试Motor_CTL类的新功能
        # print("\n🔧 开始测试Motor_CTL类...")
        # motor_ctl = Motor_CTL(dev_ch1)
        
        # # 测试同步控制模式
        # print("\n🔄 测试同步控制模式...")
        # motor_ctl.set_control_mode(Motor_CTL.SYNC_CONTROL)
        # motor_ctl.initialize_motor()
        
        # # 测试带运行时间的速度设置（运行2秒后自动停止）
        # print("\n➡️  测试同步控制模式下带运行时间的速度设置...")
        # motor_ctl.set_speed(100, run_time_ms=2000)  # 速度500，运行2秒
        
        # # 测试异步控制模式
        # print("\n🔄 测试异步控制模式...")
        # motor_ctl.set_control_mode(Motor_CTL.ASYNC_CONTROL)
        # motor_ctl.initialize_motor()
        
        # # 测试带运行时间的速度设置（运行3秒后自动停止）
        # print("\n➡️  测试异步控制模式下带运行时间的速度设置...")
        # motor_ctl.set_speed(0,run_time_ms=3000, left_speed=100, right_speed=-100)  # 速度300，运行3秒
        
        # 测试不带运行时间的速度设置
        # print("\n➡️  测试不带运行时间的速度设置...")
        # motor_ctl.set_speed(100)  # 速度200，不会自动停止

        # # 创建位置控制器实例
        # motor_pos = Motor_CTL_Pos(dev_ch1)

        # # 初始化电机
        # motor_pos.initialize_motor()

        # # 设置相对位置控制
        # # motor_pos.set_position(Motor_CTL_Pos.RELATIVE_POSITION, -50, 50)

        # # 或者设置绝对位置控制
        # motor_pos.set_position(Motor_CTL_Pos.ABSOLUTE_POSITION, 5, -5)
        # 创建转矩控制器
        motor_ctl_ser = Motor_CTL_Ser(dev_ch1)

        # 同步控制
        motor_ctl_ser.set_torque(Motor_CTL_Ser.SYNC_CONTROL, 1000)

        # 异步控制（分别设置左右转矩）
        # motor_ctl_ser.set_torque(Motor_CTL_Ser.ASYNC_CONTROL, 500, left_torque=800, right_torque=200)

        # 负数转矩
        # motor_ctl_ser.set_torque(Motor_CTL_Ser.SYNC_CONTROL, -500)

    except KeyboardInterrupt:
        print("\n⚠️ Operation interrupted by user.")
    finally:
        # 强制清理资源
        close_can_device(m_dev, dev_ch1)


